Alignment of a Flight Vehicle Based on Recursive Matrix Inversion 



Background Information 
{0001] Weapons systems have been developed that include reentry bodies that with guidance 
and navigation systems to control reentry of the body after separation from a launch vehicle. To 
accurately control the trajectory of the reentry body, the guidance and navigation system of the 
reentry body uses position, velocity and orientation information. Typically, the reentry body has 
an inertial measurement unit (IMU) to provide data to the guidance and navigation system during 
reentry. 

[0002] The launch vehicle typically also has a guidance and navigation system along with 
appropriate sensors, e.g., an IMU, etc. The IMU in the launch vehicle has its own reference 
frame; commonly called the inertial frame (I-frame). Similarly, the IMU in the reentry vehicle 
has its own reference frame; commonly referred to as the pseudo-inertial frame (P-frame). To 
allow the reentry vehicle to properly navigate after separation from the launch vehicle, flight 
systems commonly determine the orientation of the P-frame with respect to the I-frame using, 
e.g., a Kalman filter. The Kalman filter outputs information used by the reentry body's 
navigation computer to determine the vehicle position and velocity in the reference I-frame. 
[0003] Unfortunately, the Kalman filter is complex and difficult to implement. Specifically, 
the Kalman filter requires an initial estimate of the relative orientations of the I-frame and P- 
frame such that the small angle approximation is valid. A poor initial estimate could lead to 
divergence of the Kalman filter even when there is complete observability of the relative 
orientations. 

[0004] Therefore, there is a need in the art for an alignment mechanism in a flight vehicle 
that does not require an a priori estimate that is good enough for the small angle approximation 
to be valid. 



Summary 

[0005] Embodiments of the present invention provide an improved alignment mechanism for 
flight vehicles. The alignment mechanism is based on a recursive matrix inversion algorithm. In 
one embodiment, the algorithm uses a weighting fiinction to improve performance. In one 
embodiment, the weighting function is based on the magnitude of the cross product between the 
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pseudo position and velocity vector and, in another embodiment, the weighting function includes 
the angular separation between these vectors. 

[0006] In one embodiment, a method for recursively determining alignment of a flight 
vehicle during flight is provided. The method includes generating data in a reference coordinate 
frame and in a second coordinate frame at a plurality of points in time during the flight, 
recursively generating first and second matrices from the data in the reference coordinate frame 
and the second coordinate frame, and at each point in time, determining an alignment output 
based on the inverted first matrix and the second matrix. 

Brief Description of the Drawings 
|0007] Figure 1 is a data flow diagram of one embodiment of a process for determining 
alignment of a flight vehicle based on recursive matrix inversion. 

(0008] Figure 2 is a block diagram of one embodiment of a system that determines alignment 
of a flight vehicle based on recursive matrix inversion. 

Detailed Description 

[0009] In the following detailed description, reference is made to the accompanying 
drawings that form a part hereof, and in which is shown by way of illustration specific 
illustrative embodiments in which the invention may be practiced. These embodiments are 
described in sufficient detail to enable those skilled in the art to practice the invention, and it is to 
be understood that other embodiments may be utilized and that logical, mechanical and electrical 
changes may be made without departing from the spirit and scope of the present invention. The 
following detailed description is, therefore, not to be taken in a limiting sense. 

1. Introduction 

[0010] Embodiments of the present invention use a recursive matrix inversion based 
algorithm to align, for example, the P-frame of a reentry body with the I-frame of a launch 
vehicle. Advantageously, the recursive matrix inversion based algorithm does not assume a 
small angle approximation. Further, it also does not require an a priori estimate that is good 
enough for the small angle approximation to be valid. Hence, the performance of the algorithm 
is determined by the observability rather than the quality of the a priori estimate. This is an 
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improvement over existing systems using a Kalman filter since, with the Kalman filter, a poor 
initial estimate could lead to divergence of the filter even when there is complete observability. 

2. Generalized Formulation 

[001 1] To derive the alignment algorithm, let 

«/=[«/ , bj , ajxbj J (1) 
[0012] and 



[0013] where - and - are orthogonal unit vectors. 



[0014] Further define the block matrixes, 



u' N = \u[ u{ u' N \ (3) 



[0015] and 

K = [< < --- <\ (4) 

= T?[u? u> -- - u' N ]=Tru' N 

Tjl JjP 

(0016] Notice that N and N are 3X3N matrixes. Tf is a matrix that defines the 
transformation from the I-frame to the P-frame. In one embodiment, the Tf matrix is a direction 
cosine matrix defining the orientation of the P-frame with respect to the I-frame. Here the P- 
frame is the unknown to be solved. The I-frame is the known reference frame. The Tf matrix is 
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used in the navigation computer of a reentry body to determine the vehicle position and velocity 
in the I-frame. Essentially, this Tf matrix produces the same output as a Kalman filter. 
10017] Construct the 3X3 matrixes, 

W PP (N)=ljr(uZj (5) 
|0018] and 

W lp (N)=u' N (u%j (6) 
|0019] The I-to-P transformation matrix can be obtained by matrix inversion, 

T, P (N)= W PP {N)W; P X {N) (7) 
3. Recursive Formulation 

[0020] The above equations will be expressed in a recursive form to facilitate a software 
implementation. It is understood, however, that the mechanism described herein can also be 
implemented in hardware, firmware or any appropriate combination of hardware, software or 
firmware. 

[0021] By definition, 

£/L.= [".' "2 "-- K u'^]=[u' N u' N+i ] (8) 
[0022] Hence, 

W pp (N + \) = U^(u p + J=[ U N "N+ih* "£ + if = W pp {N)+u p N+ \u p N+ J (9) 
(0023] Similarly, 
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W IP (N + 1) = W IP (N) + (uj +1 f 



(10) 



[0024] The I-to-P transformation matrix that represents the P-frame alignment with respect to 
the I-frame, can be solved through inverting a 3X3 matrix as follows, 



4. Choice of a and b Vectors 

[0025] The alignment algorithm developed above involves the computation of the orthogonal 
unit vectors a and b using IMU measurements of the reentry body in the P-frame and 
measurements from a reference system in the I-frame. The orthogonal unit vectors could be the 
sensed acceleration and angular rate with the reference being the nominal trajectory. For better 
accuracy, the reference system could be the based on outputs of the IMU in the launch vehicle. 
Typically, the IMU in the launch vehicle is a stabilized platform mechanization. In one 
embodiment, angular rate is derived from gimbal resolver data. For concept evaluation, an 
algorithm evaluation test bed was developed using existing flight data from prior missile tests. 

5. Weighting Functions 

[0026] The performance characteristics presented in Figure 1 and the trajectory 
characteristics presented in Figures 2 and 3, suggest not all the data are not equally useful in 
providing the alignment information. Thus, in one embodiment, the alignment algorithm uses 
weighting on the data. Equations (9), (10), and (11) derived above become, 



T f p (N + \)= W pp {N + \)WJ p {N + \) 



(11) 




(13) 



where w N+l is the weighting function assigned to the data available at time 



[0027] 



Similarly, 
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W lp (N + 1) = W !P ( AO + w 2 N+l (u J +1 f 



(14) 



[0028] The I-to-P transformation matrix that represents the P-frame alignment with respect to 
the I-frame, remain the same as follows, 



6. Implementation System 

[0029] Figure 1 is a data flow diagram of one embodiment of a process indicated generally at 
100 for determining alignment of a flight vehicle based on recursive matrix inversion. The 
process generates data in an I-frame from a reference IMU at block 102. Typically, the reference 
IMU is located on a launch vehicle. The data is typically pseudo position and velocity data 
although other data could also be used as described above. At block 104, similar data is 
generated in the P-frame with a slave IMU, e.g., an IMU located on a reentry body. In one 
embodiment, the data at block 102 and 104 is generated every 1 second during at least a portion 
of the flight. In other embodiments, other appropriate time intervals are used. 
[0030] At blocks 106 and 108, matrices are constructed in the I and P frames, respectively, 
based on the data generated in blocks 102 and 104. These matrices are 3x3 matrices as defined 
in equations (3) and (4) above. 

[0031] At blocks 1 10 and 112, the process recursively accumulates matrices W IP and W pp . 
In one embodiment, the W ]P matrix is developed recursively according to one of equations (10) 
and (30) above. Further, the W pp .matrix is similarly developed according to one of equations (9) 
and (29) above. In one embodiment, the weighting function used with equations (30) and (29) is 
the sine of the angular separation between the pseudo position and velocity. In other 
embodiments, the weighting function is based on the magnitude of the angular separation 
between the pseudo position and velocity vectors. 

[0032] At block 1 14, the process inverts the W IP matrix is inverted. Further, at block 116, 
the I frame to P frame transformation matrix is calculated based on one of equations (11) and 
(15). In one embodiment, this matrix represents a direction cosine matrix which defines the 



Tf(N + ])= W pp (N + \)W; p ] (N + \) 



(15) 
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orientation of the P-frame with respect to the I-frame. This matrix is used in a navigation 
computer to determine the position and velocity of a reentry body in the reference 1-frame. 
[0033] Figure 2 is a block diagram of one embodiment of a flight system, indicated generally 
at 200, that determines alignment of a flight vehicle based on recursive matrix inversion.. In one 
embodiment, the flight system 200 includes a first flight vehicle 201, e.g., a launch vehicle, and a 
second flight vehicle 203, e.g., a reentry body. First flight vehicle 201 includes a master inertial 
measurement unit (IMU) 202 that generates data in a reference frame commonly referred to as 
the I-frame. The second flight vehicle 203 includes slave IMU 204 that generates data in a 
second coordinate frame referred to as the P-frame. 

[0034J The second flight vehicle 203 processes data from the reference IMU 202 and the 
slave IMU 204 at a selected interval, e.g., every second, to determine the relationship between 
the I and P frames. This processing is performed in alignment processor using recursive matrix 
inversion 206. In one embodiment, the alignment processor 206 implements equations (9) - (1 1) 
above to determine the relationship between the I and P frames. In other embodiments, the 
alignment processor uses the relationship in equations (13) - (15) to determine the relationship 
between the I and P frames. The output of the alignment processor 206 is fed to the navigation 
computer 208 of the second flight vehicle 203, e.g., a reentry body, for use in controlling the 
trajectory of the reentry body. 

[0035] The methods and techniques described here may be implemented in digital electronic 
circuitry, or with a programmable processor (for example, a special-purpose processor or a 
general-purpose processor such as a computer) firmware, software, or in combinations of them. 
Apparatus embodying these techniques may include appropriate input and output devices, a 
programmable processor, and a storage medium tangibly embodying program instructions for 
execution by the programmable processor. A process embodying these techniques may be 
performed by a programmable processor executing a program of instructions to perform desired 
functions by operating on input data and generating appropriate output. The techniques may 

i 

advantageously be implemented in one or more programs that are executable on a programmable 
system including at least one programmable processor coupled to receive data and instructions 
from, and to transmit data and instructions to, a data storage system or "machine readable 
medium," at least one input device, and at least one output device. Generally, a processor will 
receive instructions and data from a machine readable medium such as a read-only memory 

I 
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and/or a random access memory. Storage devices or machine readable medium suitable for 
tangibly embodying computer program instructions and data include all forms of non-volatile 
memory, including by way of example semiconductor memory devices, such as EPROM, 
EEPROM, and flash memory devices; magnetic disks such as internal hard disks and removable 
disks; magneto-optical disks; and DVD disks. Any of the foregoing may be supplemented by, or 
incorporated in, specially-designed application-specific integrated circuits (ASICs). 
[0036] A number of embodiments of the invention defined by the following claims have 
been described. Nevertheless, it will be understood that various modifications to the described 
embodiments may be made without departing from the spirit and scope of the claimed invention. 
Accordingly, other embodiments are within the scope of the following claims. 
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